/*
 * COPYRIGHT (C) 2011-2011
 * RE2, INC.
 *
 * Author: AM
 *
 * This file is a part of darm_example_ik, which was developed for the
 * DARPA ARM ROBOT Track C project.
 * 
 * darm_example_ik is free software:
 * you can redistribute it and/or modify it under the terms of the GNU General
 * Public License as published by the Free Software Foundation, either version
 * 3 of the License, or (at your option) any later version.
 *
 * You should have received a copy of the GNU General Public License along
 * with this package.  If not, see <http://www.gnu.org/licenses/>.
 *
 * RE2, INC. disclaims all warranties with regard to this software, including
 * all implied warranties of merchantability and fitness, in no event shall
 * RE2, INC. be liable for any special, indirect or consequential damages or
 * any damages whatsoever resulting from loss of use, data or profits, whether
 * in an action of contract, negligence or other tortious action, arising out
 * of or in connection with the use or performance of this software.
 *
 */


#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sstream>
#include <ros/ros.h>

// message to support the kinematics solver
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
// #include <kinematics_msgs/GetKinematicSolverInfo.h>
// #include <kinematics_msgs/KinematicSolverInfo.h>

// message includes for DARPA ARM messages
#include <WAM_msgs/ArmPositionProperties.h>
#include <WAM_msgs/ArmVelocityProperties.h>
#include <WAM_msgs/ArmTorqueProperties.h>
#include <WAM_msgs/ArmTorques.h>
#include <WAM_msgs/ArmVelocities.h>
#include <WAM_msgs/WAMEnums.h>

#include <BHand_msgs/SetHandVelocities.h>
#include <BHand_msgs/SetHandPositions.h>
#include <BHand_msgs/SetHandMode.h>
#include <BHand_msgs/GotoHomePosition.h>
#include <BHand_msgs/BHandEnums.h>
#include <BHand_msgs/BHandProperties.h>
#include <BHand_msgs/BHandVelocities.h>
#include <BHand_msgs/BHandStrainGauge.h>

#include <DPPanTilt_msgs/ReportStaticInfo.h>
#include <DPPanTilt_msgs/HaltMotion.h>
#include <DPPanTilt_msgs/Reset.h>
#include <DPPanTilt_msgs/SetAbsolutePositionPan.h>
#include <DPPanTilt_msgs/SetAbsolutePositionTilt.h>
#include <DPPanTilt_msgs/SetSpeedForPositionMovesPan.h>
#include <DPPanTilt_msgs/SetSpeedForPositionMovesTilt.h>
#include <DPPanTilt_msgs/SetRelativePosition.h>
#include <DPPanTilt_msgs/SetVelocityPan.h>
#include <DPPanTilt_msgs/SetVelocityTilt.h>
#include <DPPanTilt_msgs/StartPositionControl.h>
#include <DPPanTilt_msgs/StartVelocityControl.h>

#include <TestExecution_msgs/TestComplete.h>

#include "re2darmSimExampleIK/DarmExampleIK.h"


int darmExampleIK::init()
{ 
  
  {  // load robot model from parameter server to get joint limits to seed IK solver  
    std::string urdf_xml, full_urdf_xml;
    // robot urdf is loaded into parameter robot_description
    m_nodeHandle.param("urdf_xml",urdf_xml,std::string("robot_description"));
    m_nodeHandle.searchParam(urdf_xml,full_urdf_xml);
    ROS_DEBUG("Reading xml file from parameter server");
    std::string result;
    if (!m_nodeHandle.getParam(full_urdf_xml, result)) {
      ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
      return false;
    }

    // initialize m_robotModel from urdf
    if (!m_robotModel.initString(result)) {
      ROS_FATAL("Could not initialize robot model");
      return false;
    }
  }

  if (m_nodeHandle.hasParam("/darm_example_side")) {
    std::string sideParam;
    m_nodeHandle.getParam("/darm_example_side", sideParam);
    if (sideParam == "Right")
      m_solveSide = RE2_RIGHTSIDE;
    else
      m_solveSide = RE2_LEFTSIDE;
  }
  else 
    m_solveSide = RE2_RIGHTSIDE;
  ROS_INFO("Running on %s side.", (m_solveSide==RE2_LEFTSIDE?"left":"right"));

  // topic namespace
  std::string m_topicNamespace;
  // kinematics namespace
  std::string m_kinematicsNamespace;
  if (m_solveSide == RE2_LEFTSIDE)
  {
      m_topicNamespace = "LeftArm";
      m_kinematicsNamespace = "darm_kinematics_left_arm";
  }
  else
  {
      m_topicNamespace = "RightArm";
      m_kinematicsNamespace = "darm_kinematics_right_arm";
  }

  
  if (0) {
    // print out model joints
    std::string result;
    std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator jointIter;
    for (jointIter = m_robotModel.joints_.begin(); jointIter != m_robotModel.joints_.end(); jointIter++)
      result += (*jointIter).first + " ";
    ROS_INFO_STREAM("Robot joints: " << result);
  }

  // make sure the inverse kinematics service is running
  // this is launched from the darm_ik launch file, and the service
  // is set up to solve the chain from baseLink to DarmSim/LeftArm/WAM/palmLink
  ros::service::waitForService("/" + m_kinematicsNamespace + "/get_ik");

  // client setup for kinematics solver
  m_clientIK = m_nodeHandle.serviceClient<kinematics_msgs::GetPositionIK>("/" + m_kinematicsNamespace + "/get_ik");
  m_clientFK = m_nodeHandle.serviceClient<kinematics_msgs::GetPositionFK>("/" + m_kinematicsNamespace + "/get_fk");

  // hard coded m_jointNames that we want to solve for, should be able to get them from the m_robotModel
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/YawJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/ShoulderPitchJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/ShoulderYawJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/ElbowJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/UpperWristYawJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/UpperWristPitchJoint");
  m_jointNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/LowerWristYawJoint");


  { // set up IK request service message to make initial
    // seed state halfway between the joint limits for each joint
    boost::shared_ptr<const urdf::Joint> joint;
      
    // coordinate frame for desired pose 
    m_requestIK.ik_request.pose_stamped.header.frame_id = "baseLink"; 

   // using the m_jointNames list as the list of joints we want to solve for
    m_requestIK.ik_request.ik_seed_state.joint_state.name = m_jointNames;

    m_requestIK.ik_request.ik_seed_state.joint_state.position.resize(m_jointNames.size());

    for (size_t j = 0; j < m_jointNames.size(); ++j) {
      joint = m_robotModel.getJoint(m_jointNames[j]);
    
      // seed solver halfway between the limits
      m_requestIK.ik_request.ik_seed_state.joint_state.position[j] = 
	(joint->limits->upper + joint->limits->lower) / 2.; 
    }
  }

  {  // set up FK request to get link pose in baseLink coordinate frame
    // this is the link origin of the last link of the manipulator
    m_linkNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/LowerWristFTPalmLink");
    // this link is fixed to the previous one and is located on the surface
    // of the palm of the hand
    m_linkNames.push_back("DarmSim/" + m_topicNamespace + "/WAM/palmLink");

    // coordinate frame to return link pose in
    m_requestFK.header.frame_id = "baseLink";

    m_requestFK.fk_link_names = m_linkNames;
      
    // joint angles to perform forward kinematics on
    m_requestFK.robot_state.joint_state.name = m_jointNames;
    m_requestFK.robot_state.joint_state.position.resize(7);      
  }



  // setup publisher to send wam positions
  m_posPub  = m_nodeHandle.advertise<WAM_msgs::SetJointPositions>("/" + m_topicNamespace + "/WAM/set_joint_positions", 1 );
  // set WAM mode every time we send a position in case user changes the
  // mode with the Dashboard
  m_modePub = m_nodeHandle.advertise<WAM_msgs::SetMode>("/" + m_topicNamespace + "/WAM/set_mode", 1 );

  // publisher to send hand commands
  m_velPubHand  = m_nodeHandle.advertise<BHand_msgs::SetHandVelocities>("/" + m_topicNamespace + "/BHand/set_hand_velocities", 1 );
  m_modePubHand = m_nodeHandle.advertise<BHand_msgs::SetHandMode>("/" + m_topicNamespace + "/BHand/set_hand_mode", 1 );
  m_posPubHand  = m_nodeHandle.advertise<BHand_msgs::SetHandPositions>("/" + m_topicNamespace + "/BHand/set_hand_positions", 1 );
  m_homePubHand = m_nodeHandle.advertise<BHand_msgs::GotoHomePosition>("/" + m_topicNamespace + "/BHand/goto_home_position", 1 );
      
  // subscriber to get arm properties
  m_propsSub  = m_nodeHandle.subscribe(
				       "/"+ m_topicNamespace + "/WAM/arm_properties", 1, &darmExampleIK::propsCallback,this);
  // initial wam power on mode
  m_wamMode = WAMMODE_INITIALIZATION;

  // subscriber to get arm positions
  m_posSub      = m_nodeHandle.subscribe("/"+ m_topicNamespace + "/WAM/arm_positions" , 1, 
					 &darmExampleIK::posCallback,this);

  // subscriber to get hand positions
  m_posHandSub  = m_nodeHandle.subscribe("/"+ m_topicNamespace + "/BHand/bhand_positions" , 1, 
					 &darmExampleIK::posHandCallback,this);
  m_currentPositionsBHand.resize(BHAND_DOF);

  // subscriber to get hand positions
  m_strainHandSub  = m_nodeHandle.subscribe("/"+ m_topicNamespace + "/BHand/bhand_strain_gauge" , 1,
                     &darmExampleIK::strainHandCallback,this);
  m_currentStrainsBHand.resize(BHAND_DOF);
  
  // Head Publishers and Subscribers
  {
    m_topicNamespace = "UpperPanTilt";
    m_startUpperPositionControlPublisher           =
      m_nodeHandle.advertise<DPPanTilt_msgs::StartPositionControl>("/" + m_topicNamespace + "/" + "start_position_control", 1 );
    m_setUpperAbsolutePositionPanPublisher  =
      m_nodeHandle.advertise<DPPanTilt_msgs::SetAbsolutePositionPan>("/" + m_topicNamespace + "/" + "set_absolute_position_pan", 1 );
    m_setUpperAbsolutePositionTiltPublisher =
      m_nodeHandle.advertise<DPPanTilt_msgs::SetAbsolutePositionTilt>("/" + m_topicNamespace + "/" + "set_absolute_position_tilt", 1 );
    
    m_reportUpperDynamicInfoSubscriber      = 
      m_nodeHandle.subscribe("/" + m_topicNamespace + "/" + "report_dynamic_info", 10, &darmExampleIK::upperDynamicInfoCallback, this );
    m_upperDynamicData.resize(4);
  }
  {
    m_topicNamespace = "LowerPanTilt";
    m_startLowerPositionControlPublisher           =
      m_nodeHandle.advertise<DPPanTilt_msgs::StartPositionControl>("/" + m_topicNamespace + "/" + "start_position_control", 1 );
    m_setLowerAbsolutePositionPanPublisher  =
      m_nodeHandle.advertise<DPPanTilt_msgs::SetAbsolutePositionPan>("/" + m_topicNamespace + "/" + "set_absolute_position_pan", 1 );
    m_setLowerAbsolutePositionTiltPublisher =
      m_nodeHandle.advertise<DPPanTilt_msgs::SetAbsolutePositionTilt>("/" + m_topicNamespace + "/" + "set_absolute_position_tilt", 1 );
    
    m_reportLowerDynamicInfoSubscriber      = 
      m_nodeHandle.subscribe("/" + m_topicNamespace + "/" + "report_dynamic_info", 10, &darmExampleIK::lowerDynamicInfoCallback, this );
    m_lowerDynamicData.resize(4);
  }

  {
    m_reportTestCompletePublisher = m_nodeHandle.advertise<TestExecution_msgs::TestComplete>("/TestExecution/test_complete", 1 );

    m_startTestSubscriber         = m_nodeHandle.subscribe( "/TestExecution/start_test", 1,
							    &darmExampleIK::startTestCallback, this );
    m_stopTestSubscriber          = m_nodeHandle.subscribe( "/TestExecution/stop_test", 1,
							    &darmExampleIK::stopTestCallback, this );
  }

  {
    m_modelstatesSubscriber       = m_nodeHandle.subscribe( "/gazebo/model_states", 1,
							    &darmExampleIK::modelStatesCallback, this );
    
    m_modelStateValid = false;
  }

  {
    m_objectRecognizerSub         = m_nodeHandle.subscribe( "/SimpleSphereDetector/SphereCoefficients", 1,
							    &darmExampleIK::objectRecognizerCallback, this );
    
    m_objectStateValid = false;

  }
  return true;
}

void darmExampleIK::modelStatesCallback(const gazebo::ModelStatesConstPtr& msg )
{
  m_modelStateValid = false;
  size_t coffeeIndex = 0;
  for (size_t j = 0; j < msg->name.size(); j++) {
    if (msg->name[j].find("cup") != std::string::npos) {
      coffeeIndex = j;
      m_modelStateValid = true;
      break;
    }
  }

  if (m_modelStateValid == false) return;

  {
    geometry_msgs::PoseStamped cupPoseInWorldFrame, cupPoseInRobotFrame;

    cupPoseInWorldFrame.header.frame_id = "world";
    cupPoseInWorldFrame.header.stamp    = ros::Time();
    cupPoseInWorldFrame.pose            = msg->pose[coffeeIndex];
    try {
      m_tfListener.transformPose("baseLink",
				 cupPoseInWorldFrame, cupPoseInRobotFrame);
    } catch (...) {
      ROS_ERROR("Could not transform cup pose to frame: baseLink");
      return;
    }
    m_cupPoseInRobotFrame  = cupPoseInRobotFrame.pose;

  }	

  ROS_DEBUG_STREAM(m_cupPoseInRobotFrame);
}

void darmExampleIK::objectRecognizerCallback(const geometry_msgs::QuaternionStampedConstPtr& msg)
{
  boost::unique_lock<boost::mutex> lock(m_objectRecognizerMutex);
  m_objectStateValid = false;

  if (msg->quaternion.w > 0.023 && msg->quaternion.w < 0.035) {
    m_objectStateValid = true;
  }

  if (m_objectStateValid == false) return;

  {
    geometry_msgs::PoseStamped objectPose, objectPoseInRobotFrame;

    objectPose.header.frame_id      = msg->header.frame_id;
    objectPose.header.stamp         = ros::Time();
    objectPose.pose.position.x      = msg->quaternion.x;
    objectPose.pose.position.y      = msg->quaternion.y;
    objectPose.pose.position.z      = msg->quaternion.z;
    objectPose.pose.orientation.x   =  0.5; 
    objectPose.pose.orientation.y   = -0.5;
    objectPose.pose.orientation.z   = -0.5;
    objectPose.pose.orientation.w   =  0.5;

    try {
      m_tfListener.transformPose("baseLink",
				 objectPose, objectPoseInRobotFrame);
    } catch (...) {
      ROS_ERROR("Could not transform object pose to frame: baseLink");
      return;
    }
    
    m_objectPoseInRobotFrame  = objectPoseInRobotFrame.pose;

  }	

  ROS_DEBUG_STREAM(m_objectPoseInRobotFrame);
}

void darmExampleIK::startTestCallback(const TestExecution_msgs::StartTestConstPtr& msg )
{
  if (!m_runningCupDemo) {
    // launch with thread so other callback functions are handled
    this->m_cupThread = new boost::thread( &darmExampleIK::pickUpCup, this);
  }
}

void darmExampleIK::stopTestCallback(const TestExecution_msgs::StopTestConstPtr& msg )
{
  if (m_runningCupDemo) {
    this->m_runningCupDemo = false;
    
    ros::Duration(.01).sleep();

    this->m_cupThread->join();
    { // stop and hold arm at most recent position
      boost::unique_lock<boost::mutex> lock(m_posSubMutex);
	
      // publish the arm joint angle command
      m_posPub.publish(m_currentPositionsWAM.wamPositions);

      { // since we sent a current position to the arm, update the seed 
	// state with the current position to minimize move distances
	for(int i = 0;i < WAM_DOF;i++) {
	  m_requestIK.ik_request.ik_seed_state.joint_state.position[i] = 
	    m_currentPositionsWAM.wamPositions[i];
	}
      }
    }
    fprintf(stderr, "\nStopped arm!\n");
  }

}

void darmExampleIK::propsCallback(const WAM_msgs::ArmPropertiesConstPtr& msg) {
  m_wamMode = (wamMode)msg->wamMode;
}

int darmExampleIK::getIK(std::vector<double>&jointAngles, const geometry_msgs::Pose& pose)
{
  // set up message with requested pose, pose in xyz, quaternion form
  m_requestIK.ik_request.pose_stamped.pose = pose;
	  
  bool ik_service_call = false;
  int callCount = 0;
  WAM_msgs::SetJointPositions originalSeedState;

  // save original seed state in case the first IK call doesn't find a solution.  We will move in steps between the
  // original seed state to the limits, alternating further moves towards the lower and upper limits
  originalSeedState.wamPositions.resize(WAM_DOF);
  for (size_t j = 0; j < m_jointNames.size(); ++j) 
    originalSeedState.wamPositions[j] =  m_requestIK.ik_request.ik_seed_state.joint_state.position[j];
  
  int lastCall = -1;
  
  while (lastCall == -1 && callCount < 11) {
    ROS_DEBUG("IK seed: (%.3f %.3f %.3f %.3f %.3f %.3f %.3f)",
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[0],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[1],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[2],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[3],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[4],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[5],
	      m_requestIK.ik_request.ik_seed_state.joint_state.position[6]);

    ik_service_call = m_clientIK.call(m_requestIK,m_responseIK);
    ++callCount;
    if(ik_service_call) {  // successful call to IK service
      for(unsigned int i=0; i < m_responseIK.solution.joint_state.name.size(); i++) {
	// if we get in here, then it found a solution, set lastCall to RE2_LAST_CALL_IK
	// IK service only fills out the joint_state.name in the response
	// if it finds a solution
	lastCall = RE2_LAST_CALL_IK; 
	ROS_INFO("joint: %s, value: %f",
		 m_responseIK.solution.joint_state.name[i].c_str(),
		 m_responseIK.solution.joint_state.position[i]);
      }
    }
    else {
      ROS_ERROR("IK Call was unsuccessful");
      return false;
    }
    if (lastCall == -1) { // we did not found a solution
      // change seed value 
      boost::shared_ptr<const urdf::Joint> joint;
      
      for (size_t j = 0; j < m_jointNames.size(); ++j) {
	joint = m_robotModel.getJoint(m_jointNames[j]);
	
	if (callCount % 2 == 0) { // move seed in positive direction in 20% steps from current to limit
	  m_requestIK.ik_request.ik_seed_state.joint_state.position[j] = 
	    originalSeedState.wamPositions[j] +
	    (joint->limits->upper - originalSeedState.wamPositions[j]) * ((double)callCount)/10.;
	}
	else {  // move seed in negative direction
	  m_requestIK.ik_request.ik_seed_state.joint_state.position[j] = 
	    originalSeedState.wamPositions[j] +
	    (joint->limits->lower - originalSeedState.wamPositions[j]) * ((double)(callCount+1.))/10.;
	}
      }
    } 
  }

  if (lastCall == RE2_LAST_CALL_IK) {
    jointAngles = m_responseIK.solution.joint_state.position;
  }
  return lastCall;
}

int darmExampleIK::getFK(geometry_msgs::Pose& pose, const std::vector<double>&jointAngles)
{
  // not currently implemented

  return false;
}

void darmExampleIK::sendPosHand(const std::vector<float>& pos, bool wait, float epsilon)
{
    
  BHand_msgs::SetHandPositions positions;

  positions.bhandPositions = pos;

  // limit commanded positions to be in range of the hand
  float limit[2];
  for (size_t j = 0; j < (size_t)BHAND_DOF; ++j) {
    if (j == BHAND_SPREAD) {
      limit[0] = 0;
      limit[1] = 3.14;
    }
    else {
      limit[0] = 0;
      limit[1] = 2.7;
    }
    positions.bhandPositions[j] = (positions.bhandPositions[j] > limit[1]?limit[1]:
				   positions.bhandPositions[j] < limit[0]?limit[0]:
				   positions.bhandPositions[j]);
  }
  modeActivatedHand(BHANDMODE_POSITION_TRPZ);

  m_posPubHand.publish(positions);
  
  if (wait) {  // do not return until we reach target pose
    size_t j;
    unsigned int areWeThereYet = false;
    uint maxStrainOffset = 400;
    std::vector<float> initialStrainsBHand;
    initialStrainsBHand.resize(BHAND_DOF);
    for(unsigned int i = 0; i < BHAND_DOF; i++)
    {
        initialStrainsBHand[i] = m_currentStrainsBHand[i];
    }
    ros::Rate r(100);
    int printCount = 0;
    /* Record the starting time, as position moves should not take more than 2 seconds, so use that as a timeout */
    ros::Time started = ros::Time::now();
    ros::Duration timeout(2);

    while (!areWeThereYet) {
      {
	// mutex locking to make sure m_currentPositionsWAM doesn't change while we are checking it      
    boost::unique_lock<boost::mutex> lockPositions(m_posHandSubMutex);
    boost::unique_lock<boost::mutex> lockStrains(m_strainHandSubMutex);
	
	for (j = 0; j < (size_t)BHAND_DOF; ++j) {
	  if (fabs(positions.bhandPositions[j] - m_currentPositionsBHand[j]) < epsilon)
	    ++areWeThereYet;  // increment for each joint at goal
	}
      } // release the lock by going out of scope
      if (areWeThereYet == BHAND_DOF || (ros::Time::now() - started) > timeout)
	return;
      else
	areWeThereYet = false;
    for(unsigned int i = 0; i < BHAND_DOF; i++)
    {
      if(fabs(m_currentStrainsBHand[i] - initialStrainsBHand[i]) > maxStrainOffset)
      {
        ROS_ERROR("Strain limit hit on finger: %d", i);
        for(unsigned int j = 0; j < BHAND_DOF; j++)
		{
            positions.bhandPositions[j] = m_currentPositionsBHand[j];
        }
		//publish current positions of the hand so it stops
        m_posPubHand.publish(positions);
      }
      return;
    }    
      ++printCount;
      if (printCount % 10 == 0) {
	fprintf(stderr, "BHand Joint Positions: (%.3f %.3f %.3f %.3f) Strains: (%.3d %.3d %.3d %.3d)\r",
		m_currentPositionsBHand[0],
		m_currentPositionsBHand[1],
		m_currentPositionsBHand[2],
		m_currentPositionsBHand[3],
        m_currentStrainsBHand[0],
        m_currentStrainsBHand[1],
        m_currentStrainsBHand[2],
        m_currentStrainsBHand[3]);
	printCount = 0;
      }
      r.sleep();  // sleep a little so we don't hog too much processor time
    }
  }
  
}

void darmExampleIK::sendVelHand(const std::vector<float>& vel) {
  BHand_msgs::SetHandVelocities velocities;

  velocities.bhandVelocities = vel;
  modeActivatedHand(BHANDMODE_VELOCITY);
  m_velPubHand.publish(velocities);
}


void darmExampleIK::stopVelHand() {
  BHand_msgs::SetHandVelocities velocities;

  velocities.bhandVelocities.resize(BHAND_DOF);
  velocities.bhandVelocities[BHAND_FINGERONE]     = 0;
  velocities.bhandVelocities[BHAND_FINGERTWO]     = 0;
  velocities.bhandVelocities[BHAND_FINGERTHREE]   = 0;
  velocities.bhandVelocities[BHAND_SPREAD]        = 0;

  modeActivatedHand(BHANDMODE_VELOCITY);
  m_velPubHand.publish(velocities);
}

void darmExampleIK::modeActivatedHand(int index) {
  BHand_msgs::SetHandMode mode;

  mode.handMode = index;

  m_modePubHand.publish(mode);
  // give a bit of time to make sure hand controller receives the mode command
  ros::Duration(.5).sleep();
}

void darmExampleIK::homeHand() {
  BHand_msgs::GotoHomePosition msg;
  m_homePubHand.publish( msg );
  ros::Duration(.01).sleep();
}

bool darmExampleIK::moveArmIK(geometry_msgs::Pose& nextPose, bool wait, double epsilon)
{
  KDL::Rotation handRotation;// convert from RPY to quaternion
  WAM_msgs::SetMode mode;
  mode.wamMode = WAMMODE_POSITION_TRPZ;

  WAM_msgs::SetJointPositions positionsWAM;
  positionsWAM.wamPositions.resize(WAM_DOF);

  ROS_DEBUG_STREAM("Finding solution for: " << nextPose);

  m_robotPose.position = nextPose.position;

  // nextPose comes in as xyz RPY, need to change it to xyz, quaternion pose
  
  handRotation = KDL::Rotation::RPY(nextPose.orientation.x, 
				    nextPose.orientation.y,
				    nextPose.orientation.z);
  handRotation.GetQuaternion(m_robotPose.orientation.x,
			     m_robotPose.orientation.y,
			     m_robotPose.orientation.z,
			     m_robotPose.orientation.w);
	    
  std::vector<double> ikAngles;
  int lastCall = getIK(ikAngles, m_robotPose);

  if (lastCall == RE2_LAST_CALL_IK) {  // successfull IK call, move arm

    m_modePub.publish(mode);  // change to trapezoidal mode
    ros::Duration(.5).sleep();  // let mode message get through before sending new joint angles
    
    for(int i = 0;i < WAM_DOF;i++) {  // put IK angles into msg for WAM
      positionsWAM.wamPositions[i] = m_responseIK.solution.joint_state.position[i];
    }
    
    // publish the arm joint angle command
    m_posPub.publish(positionsWAM);

    { // since we sent a current position to the arm, update the seed 
      // state with the current position to minimize move distances
      for(int i = 0;i < WAM_DOF;i++) {
	m_requestIK.ik_request.ik_seed_state.joint_state.position[i] = 
	  positionsWAM.wamPositions[i];
      }
    }
    
  }
  else {
    ROS_WARN_STREAM("Couldn't get IK solution for:" << nextPose);
    return false;
  }

  if (wait) {  // do not return until we reach target pose
    size_t j;
    int areWeThereYet = 0;
    ros::Rate r(100);
    int printCount = 0;
    while (areWeThereYet < WAM_DOF) {
      {
	// mutex locking to make sure m_currentPositionsWAM doesn't change while we are checking it      
	boost::unique_lock<boost::mutex> lock(m_posSubMutex);

	for (j = 0; j < (size_t)WAM_DOF; ++j) {
	  if (fabs(positionsWAM.wamPositions[j] - m_currentPositionsWAM.wamPositions[j]) < epsilon)
	    ++areWeThereYet;  // increment for each joint at goal
	}
      } // release the lock by going out of scope
      if (areWeThereYet == WAM_DOF)
	return true;
      else
	areWeThereYet = 0;
      ++printCount;
      if (printCount % 10 == 0) {
	fprintf(stderr, "WAM Joint Angles: (%.3f %.3f %.3f %.3f %.3f %.3f)\r",
		m_currentPositionsWAM.wamPositions[0],
		m_currentPositionsWAM.wamPositions[1],
		m_currentPositionsWAM.wamPositions[2],
		m_currentPositionsWAM.wamPositions[3],
		m_currentPositionsWAM.wamPositions[4],
		m_currentPositionsWAM.wamPositions[5]);
	printCount = 0;
      }
      r.sleep();  // sleep a little so we don't hog too much processor time
    }
  }
  return true;
}

bool darmExampleIK::moveArmFK(const std::vector<float>&jointAngles, bool wait, double epsilon)
{
  WAM_msgs::SetMode mode;
  mode.wamMode = WAMMODE_POSITION_TRPZ;

  WAM_msgs::SetJointPositions positionsWAM;

  positionsWAM.wamPositions.resize(WAM_DOF);

  for(unsigned int i=0; i < m_requestFK.robot_state.joint_state.position.size(); i++) {
    m_requestFK.robot_state.joint_state.position[i] = positionsWAM.wamPositions[i] = 
      jointAngles[i];
  }	  
  bool fk_service_call = m_clientFK.call(m_requestFK,m_responseFK);
  int lastCall = -1;
  if(fk_service_call)
    {
      for(unsigned int i=0; i < m_responseFK.pose_stamped.size(); i++) {
	// pose_stamped is filled in only if it finds a solution
	lastCall = RE2_LAST_CALL_FK;
	ROS_INFO("joint: %s, value: (%.3f, %.3f, %.3f) (%.3f, %.3f, %.3f, %.3f)",
		 m_responseFK.fk_link_names[i].c_str(),
		 m_responseFK.pose_stamped[i].pose.position.x,
		 m_responseFK.pose_stamped[i].pose.position.y,
		 m_responseFK.pose_stamped[i].pose.position.z,
		 m_responseFK.pose_stamped[i].pose.orientation.x,
		 m_responseFK.pose_stamped[i].pose.orientation.y,
		 m_responseFK.pose_stamped[i].pose.orientation.z,
		 m_responseFK.pose_stamped[i].pose.orientation.w);
      }
    }
  else
    {
      ROS_ERROR("FK Call was unsuccessful");
    }

  if (lastCall == RE2_LAST_CALL_FK) {  // successfull FK call, move arm

    m_modePub.publish(mode);  // change to trapezoidal mode
    ros::Duration(.5).sleep();  // let mode message get through before sending new joint angles
    
    // publish the arm joint angle command
    m_posPub.publish(positionsWAM);

    { // since we sent a current position to the arm, update the seed 
      // state with the current position to minimize move distances
      for(int i = 0;i < WAM_DOF;i++) {
	m_requestIK.ik_request.ik_seed_state.joint_state.position[i] = 
	  positionsWAM.wamPositions[i];
      }
    }
    
  }
  else {
    ROS_WARN_STREAM("Couldn't get FK solution for: ("
		    << m_requestFK.robot_state.joint_state.position[0] << " "
		    << m_requestFK.robot_state.joint_state.position[1]  << " "
		    << m_requestFK.robot_state.joint_state.position[2]  << " "
		    << m_requestFK.robot_state.joint_state.position[3]  << " "
		    << m_requestFK.robot_state.joint_state.position[4]  << " "
		    << m_requestFK.robot_state.joint_state.position[5]  << " "
		    << m_requestFK.robot_state.joint_state.position[6] << ")" ) ;
    return false;
  }

  if (wait) {  // do not return until we reach target pose
    size_t j;
    int areWeThereYet = false;
    int printCount = 0;
    ros::Rate r(100);
    while (!areWeThereYet) {
      {
	// mutex locking to make sure m_currentPositionsWAM doesn't change while we are checking it      
	boost::unique_lock<boost::mutex> lock(m_posSubMutex);
	
	for (j = 0; j < (size_t)WAM_DOF; ++j) {
	  if (fabs(positionsWAM.wamPositions[j] - m_currentPositionsWAM.wamPositions[j]) < epsilon)
	    ++areWeThereYet;  // increment for each joint at goal
	}
      } // release the lock by going out of scope
      if (areWeThereYet == WAM_DOF) 
	return true;
      else
	areWeThereYet = false;
      ++printCount;
      if (printCount % 10 == 0) {
	fprintf(stderr, "WAM Joint Angles: (%.3f %.3f %.3f %.3f %.3f %.3f)\r",
		m_currentPositionsWAM.wamPositions[0],
		m_currentPositionsWAM.wamPositions[1],
		m_currentPositionsWAM.wamPositions[2],
		m_currentPositionsWAM.wamPositions[3],
		m_currentPositionsWAM.wamPositions[4],
		m_currentPositionsWAM.wamPositions[5]);
	printCount = 0;
      }
      r.sleep();  // sleep a little so we don't hog too much processor time
    }
  }
  return true;
}

void darmExampleIK::sendPosHead(const std::vector<float>& lowerPos, const std::vector<float>& upperPos, bool wait, double epsilon)
{
  DPPanTilt_msgs::SetAbsolutePositionPan  panMsg;
  DPPanTilt_msgs::SetAbsolutePositionTilt tiltMsg;
  DPPanTilt_msgs::StartPositionControl    startMsg;

  m_startLowerPositionControlPublisher.publish( startMsg );
  m_startUpperPositionControlPublisher.publish( startMsg );
  ros::Duration(.01).sleep();  // let message get through before sending new angles

  panMsg.panPositionInRadians   = lowerPos[0];
  tiltMsg.tiltPositionInRadians = lowerPos[1];
  m_setLowerAbsolutePositionPanPublisher.publish(  panMsg );
  m_setLowerAbsolutePositionTiltPublisher.publish( tiltMsg );

  panMsg.panPositionInRadians   = upperPos[0];
  tiltMsg.tiltPositionInRadians = upperPos[1];
  m_setUpperAbsolutePositionPanPublisher.publish(  panMsg );
  m_setUpperAbsolutePositionTiltPublisher.publish( tiltMsg );
  
  if (wait) {  // do not return until we reach target pose
    size_t j;
    unsigned int areWeThereYet = false;
    ros::Rate r(100);
    int printCount = 0;
    /* Record the starting time, as position moves should not take more than 2 seconds, so use that as a timeout */
    ros::Time started = ros::Time::now();
    ros::Duration timeout(7);

    while (!areWeThereYet) {
      {
	// mutex locking to make sure data doesn't change while we are checking it      
	boost::unique_lock<boost::mutex> lock(m_headSubMutex);
	
	for (j = 0; j < 2; ++j) {
	  if (fabs(lowerPos[j] - m_lowerDynamicData[j]) < epsilon)
	    ++areWeThereYet;  // increment for each joint at goal
	  if (fabs(upperPos[j] - m_upperDynamicData[j]) < epsilon)
	    ++areWeThereYet;  // increment for each joint at goal
	}
      } // release the lock by going out of scope
      if (areWeThereYet == 4 || (ros::Time::now() - started) > timeout)
	return;
      else
	areWeThereYet = false;
      ++printCount;
      if (printCount % 10 == 0) {
	fprintf(stderr, "Head Joint Positions: Upper: (%.3f %.3f) Lower: (%.3f %.3f)\r",
		m_upperDynamicData[0], m_upperDynamicData[1],
		m_lowerDynamicData[0], m_lowerDynamicData[1]);
	printCount = 0;
      }
      r.sleep();  // sleep a little so we don't hog too much processor time
    }
  }
}


void darmExampleIK::posCallback(const WAM_msgs::ArmPositionsConstPtr& msg) 
{
  // lock the mutex while we update local copy of positions
  boost::unique_lock<boost::mutex> lock(m_posSubMutex);
  m_currentPositionsWAM.wamPositions = msg->wamPositions;

}

void darmExampleIK::posHandCallback(const BHand_msgs::BHandPositionsConstPtr& msg)
{
  // lock the mutex while we update local copy of positions
  boost::unique_lock<boost::mutex> lock(m_posHandSubMutex);

  m_currentPositionsBHand[BHAND_SPREAD]      = msg->bhandInnerLinkPositions[BHAND_SPREAD];
  m_currentPositionsBHand[BHAND_FINGERONE]   = msg->bhandInnerLinkPositions[BHAND_FINGERONE];
  m_currentPositionsBHand[BHAND_FINGERTWO]   = msg->bhandInnerLinkPositions[BHAND_FINGERTWO];
  m_currentPositionsBHand[BHAND_FINGERTHREE] = msg->bhandInnerLinkPositions[BHAND_FINGERTHREE];

}

void darmExampleIK::strainHandCallback(const BHand_msgs::BHandStrainGaugeConstPtr& msg)
{
  // lock the mutex while we update local copy of positions
  boost::unique_lock<boost::mutex> lock(m_strainHandSubMutex);

  m_currentStrainsBHand[BHAND_SPREAD]      = msg->bhandStrainGauge[BHAND_SPREAD];
  m_currentStrainsBHand[BHAND_FINGERONE]   = msg->bhandStrainGauge[BHAND_FINGERONE];
  m_currentStrainsBHand[BHAND_FINGERTWO]   = msg->bhandStrainGauge[BHAND_FINGERTWO];
  m_currentStrainsBHand[BHAND_FINGERTHREE] = msg->bhandStrainGauge[BHAND_FINGERTHREE];

}

void darmExampleIK::upperDynamicInfoCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg )
{
  // lock the mutex while we update local copy of data
  boost::unique_lock<boost::mutex> lock(m_headSubMutex);

  m_upperDynamicData[0] = msg->panPositionInRadians;
  m_upperDynamicData[1] = msg->tiltPositionInRadians;
  m_upperDynamicData[2] = msg->panVelocityInRadiansPerSec;
  m_upperDynamicData[3] = msg->tiltVelocityInRadiansPerSec;

}

void darmExampleIK::lowerDynamicInfoCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg )
{
  // lock the mutex while we update local copy of data
  boost::unique_lock<boost::mutex> lock(m_headSubMutex);

  m_lowerDynamicData[0] = msg->panPositionInRadians;
  m_lowerDynamicData[1] = msg->tiltPositionInRadians;
  m_lowerDynamicData[2] = msg->panVelocityInRadiansPerSec;
  m_lowerDynamicData[3] = msg->tiltVelocityInRadiansPerSec;

}

void darmExampleIK::keyboardLoop()
{
  

  printf("Reading from keyboard\n");
  printf("---------------------------\n");
  printf("Use 'I' to perform inverse kinematics\n");
  printf("Use 'R' to perform inverse kinematics w/RPY for wrist\n");
  printf("Use 'F' to perform forward kinematics\n");
  printf("Use 'S' to send last command to the robot\n");
  printf("Use 'O' to execute cup ball up script\n");
  printf("Use 'P' to execute cup pick up script\n");
  printf("Use 'T' to test velocity path mode\n");
  printf("for example: I .6 -.6 .6 0 0 0 1\n");

  std::string inputString;
  char lastCall = -1;

  KDL::Rotation handRotation;

  for(;;) {
    char c;
    std::cout << "> ";
    std::cin >> c;

    switch(c)
      {
      case 'i':
      case 'I':
      case 'r':
      case 'R':
	{
	  getline (std::cin, inputString);
	  if (c == 'i' || c == 'I')
	    std::stringstream(inputString) >> m_robotPose.position.x
					   >> m_robotPose.position.y
					   >> m_robotPose.position.z
					   >> m_robotPose.orientation.x
					   >> m_robotPose.orientation.y
					   >> m_robotPose.orientation.z
					   >> m_robotPose.orientation.w;
	  else { // c == 'r' || c == 'R'
	    double roll, pitch, yaw;
	    std::stringstream(inputString) >> m_robotPose.position.x
					   >> m_robotPose.position.y
					   >> m_robotPose.position.z
					   >> roll
					   >> pitch
					   >> yaw;

	    handRotation = KDL::Rotation::RPY(roll, pitch, yaw);
	    handRotation.GetQuaternion(m_robotPose.orientation.x,
				       m_robotPose.orientation.y,
				       m_robotPose.orientation.z,
				       m_robotPose.orientation.w);
	    
	  }

	  std::vector<double> ikAngles;
	  lastCall = getIK(ikAngles, m_robotPose);
	}
      break;
      case 'f':
      case 'F':
	{
	  getline (std::cin, inputString);
	  
	  std::stringstream(inputString) 
	    >> m_requestFK.robot_state.joint_state.position[0] 
	    >> m_requestFK.robot_state.joint_state.position[1] 
	    >> m_requestFK.robot_state.joint_state.position[2] 
	    >> m_requestFK.robot_state.joint_state.position[3] 
	    >> m_requestFK.robot_state.joint_state.position[4] 
	    >> m_requestFK.robot_state.joint_state.position[5] 
	    >> m_requestFK.robot_state.joint_state.position[6] ;
	  
	  bool fk_service_call = m_clientFK.call(m_requestFK,m_responseFK);
	  lastCall = -1;
	  if(fk_service_call)
	    {
	      for(unsigned int i=0; i < m_responseFK.pose_stamped.size(); i++) {
		// pose_stamped is filled in only if it finds a solution
		lastCall = RE2_LAST_CALL_FK;
		ROS_INFO("joint: %s, value: (%.3f, %.3f, %.3f) (%.3f, %.3f, %.3f, %.3f)",
			 m_responseFK.fk_link_names[i].c_str(),
			 m_responseFK.pose_stamped[i].pose.position.x,
			 m_responseFK.pose_stamped[i].pose.position.y,
			 m_responseFK.pose_stamped[i].pose.position.z,
			 m_responseFK.pose_stamped[i].pose.orientation.x,
			 m_responseFK.pose_stamped[i].pose.orientation.y,
			 m_responseFK.pose_stamped[i].pose.orientation.z,
			 m_responseFK.pose_stamped[i].pose.orientation.w);
	      }
	    }
	  else
	    {
	      ROS_ERROR("FK Call was unsuccessful");
	    }
	}
      break;
      case 's':
      case 'S':
	{
	  if (lastCall> -1) { // we have valid joint angles
	    WAM_msgs::SetJointPositions positions;

	    std::cout << "Sending last " << (lastCall == RE2_LAST_CALL_IK?"IK":"FK")
		      << " message" << std::endl;

	    {  // set wam mode to trapezoidal
	      WAM_msgs::SetMode mode;
	      mode.wamMode = WAMMODE_POSITION_TRPZ;
	      m_modePub.publish(mode);
	      ros::Duration(.5).sleep();
	    }

	    {  // fill in wam message with correct data
	      positions.wamPositions.resize(WAM_DOF);
	      for(int i = 0;i < WAM_DOF;i++) {
		positions.wamPositions[i] = (lastCall == RE2_LAST_CALL_IK?
					     m_responseIK.solution.joint_state.position[i]:
					     m_requestFK.robot_state.joint_state.position[i]);			     
	      }

	      // publish desired joint angles to wam	      
	      m_posPub.publish(positions);
	    }
	    
	    { // since we sent a current position to the arm, update the seed 
	      // state with the current position to minimize move distances
	      for(int i = 0;i < WAM_DOF;i++) {
		m_requestIK.ik_request.ik_seed_state.joint_state.position[i] = 
		  positions.wamPositions[i];
	      }
	    }
	  }
	  else {
	    std::cout << "No successful last kinematics request" << std::endl;
	  }
	}
      break;
      case 'P':
      case 'p':
	{
	  pickUpCup();
	}
      break;
      case 'O':
      case 'o':
	{
	  pickUpBall();
	}
      break;
      case 'T':
      case 't':
	{
		testVelocityPath();
	}
      break;
      case 'Q':
      case 'q':
	{
	  return;
	}
      break;
      default:
	{
	}
      }
  }
}

void darmExampleIK::pickUpBall()
{
  WAM_msgs::SetJointPositions positionsWAM;
  std::vector<float> posLower, posUpper;
  KDL::Rotation handRotation;
  geometry_msgs::Pose   ballPose;
  positionsWAM.wamPositions.resize(WAM_DOF);
  std::vector<float> handPos;
  handPos.resize(BHAND_DOF);
  
  float ballSpread = .6;

  // Move the head to starting position
  posLower.resize(2);
  posLower[0] =  0.0;
  posLower[1] =  0.0;

  posUpper.resize(2);
  if (m_solveSide == RE2_LEFTSIDE) 
    posUpper[0] =  0.5;
  else
    posUpper[0] = -0.5;
  posUpper[1] = -0.75;

  sendPosHead(posLower, posUpper);

  for ( int i = 0; i < WAM_DOF; i++ ) {
    positionsWAM.wamPositions[i] = 0.0;
  }

  {  // make sure hand is in fist shape
    {
      boost::unique_lock<boost::mutex> lock(m_posHandSubMutex);
      handPos[BHAND_SPREAD]      =  m_currentPositionsBHand[BHAND_SPREAD];
      handPos[BHAND_FINGERONE]   =  m_currentPositionsBHand[BHAND_FINGERONE];
      handPos[BHAND_FINGERTWO]   =  m_currentPositionsBHand[BHAND_FINGERTWO];
      handPos[BHAND_FINGERTHREE] =  m_currentPositionsBHand[BHAND_FINGERTHREE];
    }
      
    if (fabs(handPos[BHAND_SPREAD]) > .3) {
      // if spread is non-zero, open all fingers to make sure they don't bang into each other
      // then zero spread, then send to fist
      handPos[BHAND_SPREAD]      =  handPos[BHAND_SPREAD];
      handPos[BHAND_FINGERONE]   =  0;
      handPos[BHAND_FINGERTWO]   =  0;
      handPos[BHAND_FINGERTHREE] =  0;
      sendPosHand(handPos);
      handPos[BHAND_SPREAD]      =  0;
      handPos[BHAND_FINGERONE]   =  0;
      handPos[BHAND_FINGERTWO]   =  0;
      handPos[BHAND_FINGERTHREE] =  0;
      sendPosHand(handPos);
    }

    handPos[BHAND_SPREAD]      =  0;
    handPos[BHAND_FINGERONE]   = 2.;
    handPos[BHAND_FINGERTWO]   = 2.;
    handPos[BHAND_FINGERTHREE] = 2.;
    sendPosHand(handPos);
  }

  { // should always go to wing position at startup to be sure we don't hit the table with the arm
    ROS_INFO("Hit return to go to 'wing' position:");
    //    getchar();
    for ( int i = 0; i < WAM_DOF; i++ )
      {
	positionsWAM.wamPositions[i] = 0.0;
      }
    if (m_solveSide == RE2_LEFTSIDE) 
      positionsWAM.wamPositions[WAM_YAW_ID] =  1.5;
    else
      // The right arm needs to go opposite the left
      positionsWAM.wamPositions[WAM_YAW_ID] = -1.5;

    positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = -1.5;
    positionsWAM.wamPositions[WAM_ELBOW_ID] = 2.5;

    moveArmFK(positionsWAM.wamPositions, true, 0.05);

  }
  // Get the arm elbow out of the swiss ranger's FOV
  // positionsWAM.wamPositions[WAM_YAW_ID] =  -1.5;
  // positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = 1.5;
  // moveArmFK(positionsWAM.wamPositions, true, 0.05);

  // homeHand();

  {

    // Look for the ball up to 10 times
    for (uint i=0; i<10; ++i) {
      if (m_objectStateValid) {
	boost::unique_lock<boost::mutex> lock(m_objectRecognizerMutex);

	std::cout << "Position of object in robot reference frame is: "
		  << std::endl
		  << m_objectPoseInRobotFrame.position << std::endl;
	ballPose = m_objectPoseInRobotFrame;
	break;
      }
      else {
	ros::Duration(0.075).sleep(); // Wait to see if object will be recognized
      }
    }

    if (!m_objectStateValid) {
      ROS_WARN("couldn't find ball.");
      return;
    }
  }
  {
    double offset = 0.085;
    double roll  =  M_PI;
    double pitch =  0.;
    double yaw   =  0.;


    { // via point above ball
      m_robotPose.position.x =  ballPose.position.x;
      m_robotPose.position.y =  ballPose.position.y;
      m_robotPose.position.z =  ballPose.position.z + .3;
      m_robotPose.orientation.x = roll;
      m_robotPose.orientation.y = pitch;
      m_robotPose.orientation.z = yaw;

      if (1) {
        std::vector<double> ikAngles;

        // make sure goal is reachable
        int lastCall = getIK(ikAngles, m_robotPose);
        if(m_solveSide == RE2_RIGHTSIDE) {
          if (true || lastCall > 0) {
          {
              ROS_INFO("Going to intermediate position.");
              positionsWAM.wamPositions[WAM_YAW_ID]           = -0.529;
              positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] =  0.772;
              positionsWAM.wamPositions[WAM_SHOULDERYAW_ID]   = -1.729;
              positionsWAM.wamPositions[WAM_ELBOW_ID]         =  1.657;
              positionsWAM.wamPositions[WAM_UPPERWRISTYAW_ID] = -0.724;
              positionsWAM.wamPositions[WAM_WRISTPITCH_ID]    =  0.746;
              positionsWAM.wamPositions[WAM_LOWERWRISTYAW_ID] = -2.389;
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
          if (false) {
              positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = ikAngles[WAM_SHOULDERPITCH_ID];
              positionsWAM.wamPositions[WAM_ELBOW_ID] = ikAngles[WAM_ELBOW_ID];
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
          if (false) {
              for(int i = 0;i < WAM_DOF;i++) {  // put IK angles into msg for WAM
              positionsWAM.wamPositions[i] = m_responseIK.solution.joint_state.position[i];
              }
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
        }
        else
          return;
        }
        else
        {
          if (true || lastCall > 0) {
          {
              ROS_INFO("Going to intermediate position.");
              positionsWAM.wamPositions[WAM_YAW_ID]           =  1.57;
              positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = -0.2;
              positionsWAM.wamPositions[WAM_SHOULDERYAW_ID]   =  0.0;
              positionsWAM.wamPositions[WAM_ELBOW_ID]         =  1.0;
              positionsWAM.wamPositions[WAM_UPPERWRISTYAW_ID] =  0.5;
              positionsWAM.wamPositions[WAM_WRISTPITCH_ID]    =  0.846;
              positionsWAM.wamPositions[WAM_LOWERWRISTYAW_ID] = -2.389;
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
          if (false) {
              positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = ikAngles[WAM_SHOULDERPITCH_ID];
              positionsWAM.wamPositions[WAM_ELBOW_ID] = ikAngles[WAM_ELBOW_ID];
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
          if (false) {
              for(int i = 0;i < WAM_DOF;i++) {  // put IK angles into msg for WAM
              positionsWAM.wamPositions[i] = m_responseIK.solution.joint_state.position[i];
              }
              moveArmFK(positionsWAM.wamPositions, true, 0.05);
          }
        }
        else
          return;
        }
        
      }

      if (false) {
	for(int i = 0;i < WAM_DOF;i++) {  
	  positionsWAM.wamPositions[i] = 0.;
	}
	moveArmFK(positionsWAM.wamPositions, true, 0.05);
      }

      moveArmIK(m_robotPose);
      {
	handPos[BHAND_SPREAD]      =  0;
	handPos[BHAND_FINGERONE]   =  0;
	handPos[BHAND_FINGERTWO]   =  0;
	handPos[BHAND_FINGERTHREE] =  0;
	sendPosHand(handPos);
	handPos[BHAND_SPREAD]      = ballSpread;
	handPos[BHAND_FINGERONE]   =  0;
	handPos[BHAND_FINGERTWO]   =  0;
	handPos[BHAND_FINGERTHREE] =  0;
	sendPosHand(handPos);

      }
      
      // grasp position
      m_robotPose.position.x =  ballPose.position.x;
      m_robotPose.position.y =  ballPose.position.y;
      m_robotPose.position.z =  ballPose.position.z + offset;
      m_robotPose.orientation.x = roll;
      m_robotPose.orientation.y = pitch;
      m_robotPose.orientation.z = yaw;
      moveArmIK(m_robotPose);

      ros::Duration(0.5).sleep();

      {
	// and attempts to close fingers by sending pos = 2.3
	handPos[BHAND_SPREAD]      = ballSpread;
	handPos[BHAND_FINGERONE]   = 2.3;
	handPos[BHAND_FINGERTWO]   = 2.3;
	handPos[BHAND_FINGERTHREE] = 2.3;
	sendPosHand(handPos);
      }

      // point to drop ball from
      m_robotPose.position.x =  0.;
      m_robotPose.position.y =  ballPose.position.y;
      m_robotPose.position.z =  ballPose.position.z + .3;
      m_robotPose.orientation.x = roll;
      m_robotPose.orientation.y = pitch;
      m_robotPose.orientation.z = yaw;
      moveArmIK(m_robotPose);
      // drop ball
      {
	// open fingers
	handPos[BHAND_SPREAD]      = ballSpread;
	handPos[BHAND_FINGERONE]   =  0;
	handPos[BHAND_FINGERTWO]   =  0;
	handPos[BHAND_FINGERTHREE] =  0;
	sendPosHand(handPos);
      }
      homeHand();
    }
  }
  { // should always go to wing position when finished
    ROS_INFO("Hit return to go to 'wing' position:");

    //go to intermediate position again so arm does not hit table when moving
     ROS_INFO("Going to intermediate position.");
    positionsWAM.wamPositions[WAM_YAW_ID]           = m_currentPositionsWAM.wamPositions[WAM_YAW_ID];
    positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = 0;
    positionsWAM.wamPositions[WAM_SHOULDERYAW_ID]   = 0;
    positionsWAM.wamPositions[WAM_ELBOW_ID]         = 0;
    positionsWAM.wamPositions[WAM_UPPERWRISTYAW_ID] = 0;
    positionsWAM.wamPositions[WAM_WRISTPITCH_ID]    = 0;
    positionsWAM.wamPositions[WAM_LOWERWRISTYAW_ID] = 0;
    moveArmFK(positionsWAM.wamPositions, true, 0.05);

    
    //    getchar();
    for ( int i = 0; i < WAM_DOF; i++ )
      {
	positionsWAM.wamPositions[i] = 0.0;
      }
    if (m_solveSide == RE2_LEFTSIDE) 
      positionsWAM.wamPositions[WAM_YAW_ID] =  1.5;
    else
      // The right arm needs to go opposite the left
      positionsWAM.wamPositions[WAM_YAW_ID] = -1.5;

    positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = -1.5;
    positionsWAM.wamPositions[WAM_ELBOW_ID] = 2.5;

    moveArmFK(positionsWAM.wamPositions, false, 0.05);
    {
      handPos[BHAND_SPREAD]      =  0;
      handPos[BHAND_FINGERONE]   =  2;
      handPos[BHAND_FINGERTWO]   =  2;
      handPos[BHAND_FINGERTHREE] =  2;
      sendPosHand(handPos, false);
    }

  }
}

void darmExampleIK::pickUpCup()
{
  std::vector<geometry_msgs::Pose> robotPoses;  // robotPoses contains via points for path to get cup
  size_t pickUpIndex, midAirIndex, setDownIndex, finalPosIndex;
  std::vector<double> ikAngles;
  WAM_msgs::SetJointPositions positionsWAM;
  positionsWAM.wamPositions.resize(WAM_DOF);

  m_runningCupDemo = true;

  WAM_msgs::SetMode mode;
  mode.wamMode = WAMMODE_POSITION_TRPZ;
  m_modePub.publish(mode);
  ros::Duration(.5).sleep();

  // robotPoses holds a list of via points for the arm, along with 
  // indices to know when to perform other actions.
  // We need these via points to make sure we don't run the arm 
  // into the table while performing large moves since moves
  // are executed in joint space, not cartesian space.
  // While the moves are linear in joint space, ensuring that all
  // joints reach their goal simultaneously, they are decidedly
  // not linear in cartesian space

  geometry_msgs::Pose localPose;  // holding xyz, rpy, not quaternions
  geometry_msgs::Pose graspPose;  // holding xyz, rpy, not quaternions

  if (m_modelStateValid) {
    graspPose.position.x = m_cupPoseInRobotFrame.position.x + 0.00;
    graspPose.position.y = m_cupPoseInRobotFrame.position.y + 0.075;
    graspPose.position.z = m_cupPoseInRobotFrame.position.z + 0.125;
  }
  else {
    graspPose.position.x =  0.6 * m_solveSide;
    graspPose.position.y = -0.82;
    graspPose.position.z =  0.08663;
  }

  graspPose.orientation.x = 1.57;
  graspPose.orientation.y = 0;
  graspPose.orientation.z = 0;
  // above cup, wrist pointing towards camera
  // localPose.position.x =  0.6 * m_solveSide;
  // localPose.position.y = -0.82;
  // localPose.position.z =  0.5 + 0.18663;
  // localPose.orientation.x = 1.57;
  // localPose.orientation.y = -1.57;
  // localPose.orientation.z = 0.;

  localPose = graspPose;
  localPose.position.z += 0.5;

  robotPoses.push_back(localPose);
  // slightly lower position
  // localPose.position.z =  0.3 + 0.18663;

  localPose = graspPose;
  localPose.position.z += 0.3;

  robotPoses.push_back(localPose);
  // slightly lower position and a little behind cup so we don't hit it
  // localPose.position.y = -0.75;
  // localPose.position.z =  0.1 + 0.18663;

  localPose = graspPose;
  localPose.position.y += 0.1;
  localPose.position.z += 0.1;

  robotPoses.push_back(localPose);
  // next to cup
  // localPose.position.y = -0.82;
  // localPose.position.z = -0.1 + 0.18663;

  localPose = graspPose;

  robotPoses.push_back(localPose);
  // index of pick up position in robotPoses list
  pickUpIndex = robotPoses.size() - 1;
  // move cup straight up
  // localPose.position.z =  0.1 + 0.18663;

  localPose = graspPose;
  localPose.position.z += 0.2;

  robotPoses.push_back(localPose);
  // move cup to new position over table
  // localPose.position.x =  0.4 * m_solveSide;
  // localPose.position.y = -0.959;
  // localPose.position.z =  0.5 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.6;  // move towards center
  localPose.position.y += -0.15;
  localPose.position.z += 0.4;

  robotPoses.push_back(localPose);
  // index of mid air point in robotPoses list
  midAirIndex = robotPoses.size() - 1;
  // down and over
  // localPose.position.x =  0.2 * m_solveSide;
  // localPose.position.z =  0.1 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.3;  // move towards center
  localPose.position.y += -0.15;
  localPose.position.z += 0.1;

  robotPoses.push_back(localPose);
  // move down
  // localPose.position.z = -0.1 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.3;  
  localPose.position.y += -0.15;

  robotPoses.push_back(localPose);
  // index to let go of cup
  setDownIndex = robotPoses.size() - 1;
  // move arm back and up
  // localPose.position.y = -0.909;
  // localPose.position.z =  0.1 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.3;  
  localPose.position.y += -0.15;
  localPose.position.z += 0.1;
  
  robotPoses.push_back(localPose);
  // move arm up
  // localPose.position.z =  0.3 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.3;  
  localPose.position.y += -0.15;
  localPose.position.z += 0.3;
  
  robotPoses.push_back(localPose);
  // move arm up
  // localPose.position.z =  0.5 + 0.18663;

  localPose = graspPose;
  localPose.position.x *= 0.3;  
  localPose.position.y += -0.15;
  localPose.position.z += 0.5;
  
  robotPoses.push_back(localPose);
  finalPosIndex = robotPoses.size() - 1;

  size_t j;
	  
  ROS_INFO("Executing script to pick up coffee cup");
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to home hand:");
      //    getchar();
      homeHand();
    }
  if (m_runningCupDemo)
    { // should always go to wing position at startup to be sure we don't hit the table with the arm
      ROS_INFO("Hit return to go to 'wing' position:");
      //    getchar();
      for ( int i = 0; i < WAM_DOF; i++ )
	{
	  positionsWAM.wamPositions[i] = 0.0;
	}
      if (m_solveSide == RE2_LEFTSIDE) 
	positionsWAM.wamPositions[WAM_YAW_ID] =  1.5;
      else
	// The right arm needs to go opposite the left
	positionsWAM.wamPositions[WAM_YAW_ID] = -1.5;

      positionsWAM.wamPositions[WAM_SHOULDERPITCH_ID] = -1.5;
      positionsWAM.wamPositions[WAM_ELBOW_ID] = 2.5;

      moveArmFK((positionsWAM.wamPositions));

    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to move hand to cup:");
      for ( j = 0; j <= pickUpIndex && m_runningCupDemo; ++j) {
	//      getchar();
	moveArmIK(robotPoses[j]);
	// extra homeHand here in case the script is started before
	// the hand completes its initialization.
	// The hand initialization closes the fingers when it completes
	homeHand();
	if (j < pickUpIndex) 
	  ROS_INFO("Hit return for next step: ");
      }
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to move head: ");
      //    getchar();
      std::vector<float> posLower, posUpper;
	    
      posLower.resize(2);
      posLower[0] =  0.;
      posLower[1] =  0.;

      posUpper.resize(2);
      posUpper[0] =  0.25;
      posUpper[1] = -0.75;

      sendPosHead(posLower, posUpper);
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to grasp cup: ");
      //    getchar();
      std::vector<float> pos;
	    
      // hand has fingers directly opposite (spread = 0),
      // and attempts to close fingers by sending pos = 2.3
      pos.resize(BHAND_DOF);
      pos[BHAND_SPREAD]    =  0;
      pos[BHAND_FINGERONE] = 2.3;
      pos[BHAND_FINGERTWO] = 2.3;
      pos[BHAND_FINGERTHREE] = 2.3;
      sendPosHand(pos);
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to lift cup: ");
      for ( ; j <= midAirIndex && m_runningCupDemo; ++j) {
	//      getchar();
	moveArmIK(robotPoses[j]);
	if (j < midAirIndex) 
	  ROS_INFO("Hit return for next step: ");
      }
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to lower cup: ");
      for ( ; j <= setDownIndex && m_runningCupDemo; ++j) {
	//      getchar();
	if (j != setDownIndex)
	  moveArmIK(robotPoses[j]);
	else { // j == setDownIndex, which is right above the table
	  // epsilon is larger than the default here, because cup may
	  // have shifted in hand and the hand may not be able to
	  // reach the goal as readily if the cup hits the table 
	  moveArmIK(robotPoses[j], true, 0.1);
	}
	if (j < setDownIndex) 
	  ROS_INFO("Hit return for next step: ");
      }
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to release cup: ");
      //    getchar();
      std::vector<float> pos;
	    
      pos.resize(BHAND_DOF);
      pos[BHAND_SPREAD]    =  0;
      pos[BHAND_FINGERONE] = -0;
      pos[BHAND_FINGERTWO] = -0;
      pos[BHAND_FINGERTHREE] = -0;
      sendPosHand(pos);
    }
  if (m_runningCupDemo)
    {
      ROS_INFO("Hit return to raise arm: ");
      for ( ; j <= finalPosIndex && m_runningCupDemo; ++j) {
	//      getchar();
	moveArmIK(robotPoses[j]);
	if (j < finalPosIndex) 
	  ROS_INFO("Hit return for next step: ");
      }
    }
  std::cout << std::endl;
  {
    TestExecution_msgs::TestComplete msg;
    m_reportTestCompletePublisher.publish(msg);
  }
  m_runningCupDemo = false;
}

void darmExampleIK::testVelocityPath()
{
  std::string inputString;
  char c = 0;
  std::vector< std::vector<float> > armPositions(2, std::vector<float>(WAM_DOF));   

  std::cin.ignore( std::numeric_limits<std::streamsize>::max(), '\n' );
  for (int j = 0; j <= 1; j++) {
    std::cout << "Enter "
	      << (0 == j ? "first" : "second")
	      << " robot position > ";

    // ignore everything up to the next newline
    getline (std::cin, inputString);
    
    std::stringstream(inputString) 
      >> m_requestFK.robot_state.joint_state.position[0] 
      >> m_requestFK.robot_state.joint_state.position[1] 
      >> m_requestFK.robot_state.joint_state.position[2] 
      >> m_requestFK.robot_state.joint_state.position[3] 
      >> m_requestFK.robot_state.joint_state.position[4] 
      >> m_requestFK.robot_state.joint_state.position[5] 
      >> m_requestFK.robot_state.joint_state.position[6] ;
    
    bool fk_service_call = m_clientFK.call(m_requestFK,m_responseFK);
    if(fk_service_call) {
      for(unsigned int i=0; i < m_responseFK.pose_stamped.size(); i++) {
	// pose_stamped is filled in only if it finds a solution
	ROS_INFO("link: %s, pose: (%.3f, %.3f, %.3f) (%.3f, %.3f, %.3f, %.3f)",
		 m_responseFK.fk_link_names[i].c_str(),
		 m_responseFK.pose_stamped[i].pose.position.x,
		 m_responseFK.pose_stamped[i].pose.position.y,
		 m_responseFK.pose_stamped[i].pose.position.z,
		 m_responseFK.pose_stamped[i].pose.orientation.x,
		 m_responseFK.pose_stamped[i].pose.orientation.y,
		 m_responseFK.pose_stamped[i].pose.orientation.z,
		 m_responseFK.pose_stamped[i].pose.orientation.w);
      }
    }
    else {
      ROS_ERROR("FK Call was unsuccessful");
      return;
    }
    for (int i = 0; i < WAM_DOF; i++ ) {
      armPositions[j][i] = m_requestFK.robot_state.joint_state.position[i];
    }
  }
  {
    bool toggle = 0;

    {  // set wam mode to trapezoidal
      WAM_msgs::SetMode mode;
      mode.wamMode = WAMMODE_POSITION_TRPZ;
      m_modePub.publish(mode);
      ros::Duration(.01).sleep();
    }

    while (true) {
      std::cout << "Hit enter to go to "
		<< (0 == toggle ? "first" : "second")
		<< " robot position (q to return to main menu) > ";
      std::cin >> std::noskipws >> c;
      switch(c)
	{
	case 'Q':
	case 'q':
	  {
	    return;
	  }
	break;
	default:
	  {
	    moveArmFK(armPositions[toggle]);
	  }
	}
      
      toggle = !toggle;
    }
  }
}



